#include "seriallink_obj.h"
#include <QtSerialPort/QSerialPortInfo>
#include <QDebug>

#include "serialportwriter.h"

//add mavlink pack
#include "mavlink_help.h"

#include <string>
#include <iostream>
using namespace std;
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t * ch, uint16_t length){}


std::string string_to_hex(const std::string& input)
{
    static const char* const lut = "0123456789ABCDEF";
    size_t len = input.length();

    std::string output;
    output.reserve(2 * len);
    for (size_t i = 0; i < len; ++i)
    {
        const unsigned char c = input[i];
        output.push_back(lut[c >> 4]);
        output.push_back(lut[c & 15]);
    }
    return output;
}



string test_mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
                            uint8_t length, uint8_t crc_extra)
{
    uint16_t checksum;
    uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
    uint8_t ck[2];
    mavlink_status_t *status = mavlink_get_channel_status(chan);
    buf[0] = MAVLINK_STX;
    buf[1] = length;
    buf[2] = status->current_tx_seq;
    buf[3] = mavlink_system.sysid;
    buf[4] = mavlink_system.compid;
    buf[5] = msgid;
    status->current_tx_seq++;
    checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
    crc_accumulate_buffer(&checksum, packet, length);
    crc_accumulate(crc_extra, &checksum);
    ck[0] = (uint8_t)(checksum & 0xFF);
    ck[1] = (uint8_t)(checksum >> 8);


    string str;
    str += string((char*)buf, MAVLINK_NUM_HEADER_BYTES);
    str += string((char*)packet, length);
    str += string((char*)ck, 2);

    return str;
}


QByteArray my_mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
                            uint8_t length, uint8_t crc_extra)
{
    uint16_t checksum;
    uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
    uint8_t ck[2];
    mavlink_status_t *status = mavlink_get_channel_status(chan);
    buf[0] = MAVLINK_STX;
    buf[1] = length;
    buf[2] = status->current_tx_seq;
    buf[3] = mavlink_system.sysid;
    buf[4] = mavlink_system.compid;
    buf[5] = msgid;
    status->current_tx_seq++;
    checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
    crc_accumulate_buffer(&checksum, packet, length);
    crc_accumulate(crc_extra, &checksum);
    ck[0] = (uint8_t)(checksum & 0xFF);
    ck[1] = (uint8_t)(checksum >> 8);

    QByteArray ba;
    ba += QByteArray((const char *)buf, MAVLINK_NUM_HEADER_BYTES);
    ba += QByteArray(packet, length);
    ba += QByteArray((const char *)ck, 2);

    qDebug() << "packet length:" << length;
    qDebug() << "data length:" << ba.size();
    return ba;
}


QByteArray my_mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, mavlink_optical_flow_t &packet)
{


    // mavlink_optical_flow_t packet;
    packet.time_usec = time_usec;
    packet.flow_comp_m_x = flow_comp_m_x;
    packet.flow_comp_m_y = flow_comp_m_y;
    packet.ground_distance = ground_distance;
    packet.flow_x = flow_x;
    packet.flow_y = flow_y;
    packet.sensor_id = sensor_id;
    packet.quality = quality;


    return my_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
}

string test_mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, mavlink_optical_flow_t &packet)
{


    // mavlink_optical_flow_t packet;
    packet.time_usec = time_usec;
    packet.flow_comp_m_x = flow_comp_m_x;
    packet.flow_comp_m_y = flow_comp_m_y;
    packet.ground_distance = ground_distance;
    packet.flow_x = flow_x;
    packet.flow_y = flow_y;
    packet.sensor_id = sensor_id;
    packet.quality = quality;


    return test_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
}



int byMsgRecvThreadoInt(QByteArray bytes)
{
    int addr = bytes[0] & 0x000000FF;
    addr |= ((bytes[1] << 8) & 0x0000FF00);
    addr |= ((bytes[2] << 16) & 0x00FF0000);
    addr |= ((bytes[3] << 24) & 0xFF000000);
    return addr;
}

QByteArray intToByte(int number)
{
    QByteArray abyte0;
    abyte0.resize(4);
    abyte0[0] = (uchar)  (0x000000ff & number);
    abyte0[1] = (uchar) ((0x0000ff00 & number) >> 8);
    abyte0[2] = (uchar) ((0x00ff0000 & number) >> 16);
    abyte0[3] = (uchar) ((0xff000000 & number) >> 24);
    return abyte0;
}

int byMsgRecvThreadoShort(QByteArray bytes)
{
    int addr = bytes[0] & 0x00FF;
    addr |= ((bytes[1] << 8) & 0xFF00);
    return addr;
}

QByteArray shortToByte(int number)
{
    QByteArray abyte0;
    abyte0.resize(2);
    abyte0[0] = (uchar)  (0x00ff & number);
    abyte0[1] = (uchar) ((0xff00 & number) >> 8);
    return abyte0;
}

const int wCRC_Talbe[] =
{
    0x0000, 0xCC01, 0xD801, 0x1400, 0xF001, 0x3C00, 0x2800, 0xE401,
    0xA001, 0x6C00, 0x7800, 0xB401, 0x5000, 0x9C01, 0x8801, 0x4400
};

int CRC16(char* pyData, int wDataLen)
{
    int wCRC = 0xFFFF;
    int i;
    char yChar;

    for (i = 0; i < wDataLen; i++)
    {
        yChar = *pyData++;
        wCRC = wCRC_Talbe[(yChar ^ wCRC) & 15] ^ (wCRC >> 4);
        wCRC = wCRC_Talbe[((yChar >> 4) ^ wCRC) & 15] ^ (wCRC >> 4);
    }

    return wCRC;
}

//char cLength != 0;
QByteArray Txn_Msg_Request_Buf_Common(char cType, char cLength, QByteArray byte)
{
    QByteArray request;

    request[0]=char(0xEB);
    request[1]=char(0x90);
    request[2]=char(0xFF);
    request[3]=char(0xFF);
    request[4]=char(0xFF);
    request[5]=char(0xFF);
    request[6]=cType;
    request[7]=cLength;

    request += byte;

    return request;
}

QByteArray Txn_Msg_Request_Buf_Common_Total(char cType, char cLength, QByteArray byte)
{
    QByteArray byteTmp = Txn_Msg_Request_Buf_Common(cType,cLength, byte);
    int iCRC = CRC16((char*)byteTmp.data(), byteTmp.size());
    return byteTmp + shortToByte(iCRC);
}



seriallink_obj::seriallink_obj(QObject *parent) :
    QObject(parent)
  , serialPortWriter(0)
{
//    ros::Time::init();
    serialPort.setPortName("ttyS0");
    serialPort.setBaudRate(QSerialPort::Baud115200);
    serialPort.setDataBits(QSerialPort::Data8);
    serialPort.setFlowControl(QSerialPort::NoFlowControl);
    serialPort.setParity(QSerialPort::NoParity);
    serialPort.setStopBits(QSerialPort::OneStop);
    bool bFlag =  serialPort.open(QIODevice::ReadWrite);
    qDebug() << "open flag:"<<bFlag;
    serialPortWriter = new SerialPortWriter(&serialPort);

    qDebug() << "serial data:";
    qDebug() << serialPort.portName();
qDebug() <<serialPort.baudRate();
qDebug() << serialPort.dataBits();
qDebug() << serialPort.parity();
qDebug() << serialPort.stopBits();
qDebug() << serialPort.flowControl();

    // Q_FOREACH (QSerialPortInfo info, QSerialPortInfo::availablePorts())
    // {
    //     qDebug() << "Name : " << info.portName();
    //     qDebug() << "Description : " << info.description();
    //     qDebug() << "Manufacturer: " << info.manufacturer();
    //     qDebug() << "Serial Number: " << info.serialNumber();
    //     qDebug() << "System Location: " << info.systemLocation();

    //     QString strPortName = info.portName();
    //     if(!strPortName.isEmpty()){

    //         serialPort.setPortName(strPortName);
    //         serialPort.setBaudRate(QSerialPort::Baud115200);
    //         serialPort.setDataBits(QSerialPort::Data8);
    //         serialPort.setFlowControl(QSerialPort::NoFlowControl);
    //         serialPort.setParity(QSerialPort::NoParity);
    //         serialPort.setStopBits(QSerialPort::OneStop);
    // bool bFlag =  serialPort.open(QIODevice::ReadWrite);
    // qDebug() << "open flag:"<<bFlag;
    //         serialPortWriter = new SerialPortWriter(&serialPort);
    //     }
    // }
}

void seriallink_obj::SlotMsgReceived(const Msg& msg)
{
    int iX = msg.iX;
    int iY = msg.iY;
    long lTime = msg.lTime;
    int iQuality = msg.iQuality;

    // my_mavlink_msg_optical_flow_send(MAVLINK_COMM_0, lTime, 0,
    //     iX, iY,
    //     0, 0, iQuality, 0);

    // QByteArray writeData = Txn_Msg_Request_Buf_Common_Total(0x90, 4, shortToByte(iX)+ shortToByte(iY));
    mavlink_optical_flow_t packet;
    QByteArray writeData = my_mavlink_msg_optical_flow_send(MAVLINK_COMM_0, lTime, 0,
        iX, iY,
        0, 0, iQuality, 0, packet);  

        qDebug() << "use qt " << writeData.toHex();

    // string stdwriteData = test_mavlink_msg_optical_flow_send(MAVLINK_COMM_0, lTime, 0,
    //     iX, iY,
    //     0, 0, iQuality, 0, packet);

    // cout << "use std " << string_to_hex(stdwriteData)  << endl;

    // qDebug()<<writeData.toHex();

    if(serialPortWriter){
        serialPortWriter->write(writeData);
    }
}
